#include "PhysicsObject.h"
#include "MyString.h"


PhysicsObject::PhysicsObject(void)
{
	Init();
}

PhysicsObject::~PhysicsObject(void)
{
	if(havokRigidBody->isAddedToWorld())
	{
		RemoveObjectFromSimulation(HavokWorld::Get().physicsWorld);
	}
}

void PhysicsObject::Init()
{
	GameObject3D::Init();

	havokRigidBody = NULL;
	havokShape = NULL;

}

void PhysicsObject::Update()
{

	if( havokRigidBody != NULL &&
		(!havokRigidBody->isAddedToWorld() || 
		MyString::StringCompare(havokRigidBody->getName(),"DEAD")))
		isDead = true; // kill it

	else  // still alive
	{
		Position.x = havokRigidBody->getPosition()(0);
		Position.y = havokRigidBody->getPosition()(1);
		Position.z = havokRigidBody->getPosition()(2);
	}
}

D3DXMATRIXA16 PhysicsObject::GetTransform()
{
	D3DXMATRIXA16 translation, rotation, transform;

	hkQuaternion hkRot = havokRigidBody->getRotation();
	//hkRot.setInverse(hkRot);

	D3DXVECTOR4 rot(hkRot(0), hkRot(1), hkRot(2), hkRot(3)); 

	D3DXMatrixRotationQuaternion(&rotation, new D3DXQUATERNION(rot.x,rot.y,rot.z,rot.w));

	D3DXMatrixTranslation(&translation,Position.x,Position.y,Position.z);

	transform = rotation * translation;

	return transform;
}

void PhysicsObject::AddObjectToSimulation(hkpWorld* theWorld)
{
	CreateGeometry();
	CreatePhysicsObject();

	StartingPos = Position;

	theWorld->addEntity(havokRigidBody);
	havokRigidBody->removeReference();
	havokShape->removeReference();

	hkVector4 vel(  Velocity.x, Velocity.y, Velocity.z );
	havokRigidBody->setLinearVelocity( vel );
}

void PhysicsObject::RemoveObjectFromSimulation(hkpWorld* theWorld)
{
	theWorld->removeEntity(havokRigidBody);
}